/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import com.grt192.actuator.GRTJaguar;
import com.grt192.controller.DashBoardController;
import com.grt192.controller.breakaway.auto.BreakawayAutoDeadReckoningController;
import com.grt192.controller.breakaway.teleop.BreakawayTeleopDriveController;
import com.grt192.controller.breakaway.teleop.KickerOmegaController;
import com.grt192.controller.breakaway.teleop.RecoveryController;
import com.grt192.controller.breakaway.teleop.RollerController;
import com.grt192.controller.breakaway.test.MechanismTestController;
import com.grt192.core.GRTRobot;
import com.grt192.mechanism.CameraAssembly;
import com.grt192.mechanism.GRTDriverStation;
import com.grt192.mechanism.breakaway.GRTBreakawayRobotBase;
import com.grt192.mechanism.breakaway.KickerOmega;
import com.grt192.mechanism.breakaway.Recovery;
import com.grt192.mechanism.breakaway.Roller;
import com.grt192.sensor.GRTGyro;
import com.grt192.sensor.GRTJoystick;
import com.grt192.sensor.GRTPotentiometer;
import com.grt192.sensor.GRTSwitch;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class MainRobot extends GRTRobot {

	// Global Controllers
	protected DashBoardController dbController;
	// Autonomous Controllers
	protected BreakawayAutoDeadReckoningController autoController;
	// Teleop Controllers
	protected BreakawayTeleopDriveController driveControl;
	protected KickerOmegaController kickerController;
	protected RecoveryController recoveryController;
	protected RollerController rollerController;
	protected MechanismTestController testController;
	// Mechanisms
	protected GRTDriverStation driverStation;
	protected GRTBreakawayRobotBase robotbase;
	protected KickerOmega shooter;
	protected Roller rollers;
	protected Recovery recovery;
	protected CameraAssembly camera;

	/**
	 * Constructor for robot, where all components are created, registered with
	 * controllers and mechanisms.
	 */
	public MainRobot() {
		// Driver station components
		GRTJoystick leftJoyStick = new GRTJoystick(1, 12, "left");
		GRTJoystick rightJoyStick = new GRTJoystick(2, 12, "right");
		GRTJoystick secondaryJoyStick = new GRTJoystick(3, 15, "secondary");
		System.out.println("Joysticks Initialized");

		// PWM outputs
		GRTJaguar leftDT1 = new GRTJaguar(8);

		GRTJaguar leftDT2 = new GRTJaguar(7);

		GRTJaguar rightDT1 = new GRTJaguar(9);

		GRTJaguar rightDT2 = new GRTJaguar(10);

		GRTJaguar recovery1 = new GRTJaguar(5);
		GRTJaguar recovery2 = new GRTJaguar(4);
		GRTJaguar kicker = new GRTJaguar(2);
		GRTJaguar roller1 = new GRTJaguar(3); // left
		GRTJaguar roller2 = new GRTJaguar(6); // right
		System.out.println("Motors Initialized");

		// analog inputs
		GRTPotentiometer batterySensor = new GRTPotentiometer(7, 50,
				"batteryVoltage");
                GRTGyro gyro = new GRTGyro(1, 15, "BaseGyro");
		// digital inputs
		GRTSwitch kickerSwitch = new GRTSwitch(1, 5, "KickLimit");
		GRTSwitch recoveryUpSwitch = new GRTSwitch(2, 50, "RecoveryUp");
		GRTSwitch recoveryGroundSwitch = new GRTSwitch(7, 50, "RecoveryDown");
		System.out.println("Switches Initialized");

		// Mechanisms
		robotbase = new GRTBreakawayRobotBase(leftDT1, leftDT2, rightDT1,
				rightDT2, batterySensor, gyro);
		driverStation = new GRTDriverStation(leftJoyStick, rightJoyStick,
				secondaryJoyStick);
		shooter = new KickerOmega(kicker, kickerSwitch);
		rollers = new Roller(roller1, roller2);
		recovery = new Recovery(recovery1, recovery2, recoveryUpSwitch,
				recoveryGroundSwitch);
		System.out.println("Mechanisms Initialized");

		// camera = new CameraAssembly();
		// System.out.println("Camera Initialized");

		// Controllers
		dbController = new DashBoardController();
		dbController.start();
		System.out.println("Dashboard Initialized");

		autoController = new BreakawayAutoDeadReckoningController(robotbase,
				rollers, shooter);

		driveControl = new BreakawayTeleopDriveController(robotbase,
				driverStation);
		kickerController = new KickerOmegaController(driverStation, shooter);
		rollerController = new RollerController(driverStation, rollers);
		recoveryController = new RecoveryController(driverStation, recovery);

		testController = new MechanismTestController(rollers, shooter,
				robotbase, recovery, driverStation);
		System.out.println("Controllers Initialized");

		// Register Controllers
		autonomousControllers.addElement(autoController);

		teleopControllers.addElement(testController);

		teleopControllers.addElement(driveControl);
		teleopControllers.addElement(kickerController);
		teleopControllers.addElement(recoveryController);
		teleopControllers.addElement(rollerController);
		System.out.println("Robot initialized OK");
	}
}
